
#include <stdio.h>
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>
#include <ghor_hardware/SetMotorCmd.h>


#define IN1A 12
#define IN1B 11

#define IN2B 6
#define IN2A 7   

#define PWM1 10
#define PWM2 5


ros::NodeHandle  nh;


long sonarTimer=0;

char   frameIdSonar1[]="/sonar1";

void callbackMotor(const ghor_hardware::SetMotorCmd::Request & req, ghor_hardware::SetMotorCmd::Response & res){

   
   char buf[10];   
   sprintf(buf,"m1 : %d",req.motor1);
   nh.logerror(buf);
   
   sprintf(buf,"m2 : %d",req.motor2);
   nh.logerror(buf);
   
   sprintf(buf,"m3 : %d",req.motor3);
   nh.logerror(buf);
   
   sprintf(buf,"m4 : %d",req.motor4);
   nh.logerror(buf);
   
  
  //digitalWrite(13,HIGH);
  //delay(4000);
  //digitalWrite(13,LOW);
  
}

ros::ServiceServer<ghor_hardware::SetMotorCmd::Request,ghor_hardware::SetMotorCmd::Response> server("motorCmd",&callbackMotor);

void setup(){
 
  pinMode(13,OUTPUT); 

  initMotors();
  initRos();

}

void initMotors(){

 
  pinMode(IN1A, OUTPUT);
  pinMode(IN1B, OUTPUT);

  pinMode(IN2A, OUTPUT);
  pinMode(IN2B, OUTPUT);

  pinMode(PWM1, OUTPUT);
  pinMode(PWM2, OUTPUT); 

 
}


void initRos(){
 
  nh.initNode();
  nh.advertiseService(server);
  //nh.advertise(pub_sonar); 

  /*
  sonar_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  sonar_msg.header.frame_id =  frameIdSonar1;
  sonar_msg.field_of_view = 0.01;
  sonar_msg.min_range = 0.03;
  sonar_msg.max_range = 0.4;
  */

 
}


void loop(){

  //handleMotor(PWM1,IN1A,IN1B,80);

  //handleMotor(PWM2,IN2A,IN2B,80);

  nh.spinOnce();

}



void sendAllSonar(){

  if ((millis()-sonarTimer) > 50){

    sendSonar(0);

    sonarTimer =  millis();
  } 

}

void sendSonar(int pin){
  //sonar_msg.range = getRange(pin);
  //sonar_msg.header.stamp = nh.now();
  //pub_sonar.publish(&sonar_msg);

}


float getRange(int pin_num){
  int sample;
  // Get data
  sample = analogRead(pin_num)/4;
  // if the ADC reading is too low, 
  //   then we are really far away from anything
  if(sample < 10)
    return 254;     // max range
  // Magic numbers to get cm
  sample= 1309/(sample-3);
  return (sample - 1)/100; //convert to meters
}

void handleMotor(int pinPWM,int pinA,int pinB, long power){


  //  Serial3.print("handle   ");
  //  Serial3.println(power);

  if(power==0){

    digitalWrite(pinA, LOW);
    digitalWrite(pinB, LOW);
    analogWrite(pinPWM, 0);

  }
  else if(power>0){

    digitalWrite(pinA, LOW);
    digitalWrite(pinB, HIGH);
    analogWrite(pinPWM, abs(power));

  } 
  else{

    digitalWrite(pinA, HIGH);
    digitalWrite(pinB, LOW);
    analogWrite(pinPWM, abs(power));
  }

}





